home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 52 / Amiga Format AFCD52 (Issue 136, May 2000).iso / -serious- / programming / c / roboticsdp / examples / setposition / source / setposition.c < prev    next >
C/C++ Source or Header  |  2000-02-28  |  6KB  |  186 lines
  1. /*****************************************************************************/
  2. /* INCLUDES INCLUDES INCLUDES INCLUDES INCLUDES INCLUDES INCLUDES INCLUDES I */
  3. /*****************************************************************************/
  4. #include <clib/exec_protos.h> /* OpenLibrary & CloseLibrary */
  5. #include <stdio.h>            /* For printing error messages */
  6. #include <minissc.h>          /* minissc library definitions */
  7. #include <dos/rdargs.h>       /* For reading CLI arguments */
  8. #include <clib/dos_protos.h>  /* For reading CLI arguments */
  9.  
  10. /*****************************************************************************/
  11. /* DEFINES DEFINES DEFINES DEFINES DEFINES DEFINES DEFINES DEFINES DEFINES D */
  12. /*****************************************************************************/
  13. #define MSG_CANTOPEN    0   /* CLI message ids */
  14. #define MSG_ARGMISSING  1
  15. #define MSG_INVSERVOID  2
  16. #define MSG_INVSERVOPOS 3
  17. #define MSG_COUNT       4   /* Number of CLI messages */
  18. #define MSG_MAX_LEN     256 /* Maximum message length */
  19. /* Command line template:                                          */
  20. /* SERVO    - target servo: 0-255                                  */
  21. /* POSITION - new servo position: 0-254                            */
  22. /* HIGH     - use 9600 baud rate (default is 2400)                 */
  23. /* WIDE     - use wide motion range: 180° (default is narrow: 90°) */
  24. #define TEMPLATE        "SERVO/A/N,POSITION=POS/A/N,HIGH/S,WIDE/S"
  25. #define ARG_SERVO       0   /* Argument ids */
  26. #define ARG_POSITION    1
  27. #define ARG_HIGH        2
  28. #define ARG_WIDE        3
  29. #define ARG_COUNT       4   /* Number of arguments */
  30.  
  31. /*****************************************************************************/
  32. /* CONSTS CONSTS CONSTS CONSTS CONSTS CONSTS CONSTS CONSTS CONSTS CONSTS CON */
  33. /*****************************************************************************/
  34. const char *vertag="$VER: SetPosition 1.0 (27.1.2000)"; /* Version string */
  35.  
  36. /*****************************************************************************/
  37. /* VARIABLES VARIABLES VARIABLES VARIABLES VARIABLES VARIABLES VARIABLES VAR */
  38. /*****************************************************************************/
  39. struct MiniSSCLibrary *MiniSSCBase=NULL; /* Library base */
  40. char *message[MSG_COUNT]=
  41. {
  42.     "%s: can't open %s",
  43.     "%s: required argument missing",
  44.     "%s: invalid servo id",
  45.     "%s: invalid servo position"
  46. };                                       /* CLI messages */
  47.  
  48. /*****************************************************************************/
  49. /* MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAI */
  50. /*****************************************************************************/
  51. int main(int argc, char **argv)
  52. {
  53.     /* Variables */
  54.     struct RDArgs *arguments;      /* Command line args */
  55.     LONG value[ARG_COUNT];         /* Given argument values */
  56.     int errcode=0;                 /* Error code to CLI */
  57.     int controller=0;              /* Target controller id */
  58.     int servo=0;                   /* Target servo id */
  59.     int position;                  /* New servo position */
  60.     int rate=SSC_COMMMODE_LOW;     /* Controller's communication mode */
  61.     int range=SSC_CTRLMODE_NARROW; /* Controller's motion range */
  62.     int servorange=90;             /* Servo's motion range in degrees */
  63.     int counter;                   /* For loop counter */
  64.     char msgstring[MSG_MAX_LEN];   /* String for printing messages */
  65.  
  66.     /* Init values */
  67.     for(counter=0;counter<ARG_COUNT;counter++)
  68.     {
  69.         value[counter]=NULL;
  70.     }
  71.  
  72.     /* Open minissc library */
  73.     if(MiniSSCBase=(struct MiniSSCLibrary *)OpenLibrary(SSC_NAME,SSC_VERSION))
  74.     {
  75.         /* Read arguments */
  76.         if(arguments=ReadArgs(TEMPLATE,value,NULL))
  77.         {
  78.             /* Get servo id */
  79.             if(value[ARG_SERVO])
  80.             {
  81.                 servo=(int)*(LONG *)value[ARG_SERVO];
  82.             }
  83.             /* No servo id */
  84.             else
  85.             {
  86.                 errcode=20;
  87.             }
  88.             /* Get servo position */
  89.             if(value[ARG_POSITION])
  90.             {
  91.                 position=(int)*(LONG *)value[ARG_POSITION];
  92.  
  93.                 /* Position out of range */
  94.                 /*
  95.                    This check is not really required.
  96.                    I make the check because I don't want
  97.                    to occupy a servo for nothing.
  98.                    (Occupying causes servo to move to
  99.                    position 127.)
  100.                 */
  101.                 if((position>SSC_MAX_AVALUE)||(position<0))
  102.                 {
  103.                     sprintf(msgstring,message[MSG_INVSERVOPOS],argv[0]);
  104.                     printf("%s\n",msgstring);
  105.                     errcode=20;
  106.                 }
  107.             }
  108.             /* No position */
  109.             else
  110.             {
  111.                 errcode=20;
  112.             }
  113.             /* 9600 baud rate selected */
  114.             if(value[ARG_HIGH])
  115.             {
  116.                 rate=SSC_COMMMODE_HIGH;
  117.             }
  118.             /* 180° motion range selected */
  119.             if(value[ARG_WIDE])
  120.             {
  121.                 range=SSC_CTRLMODE_WIDE;
  122.                 servorange=180;
  123.             }
  124.  
  125.             /* Move servo */
  126.             if(errcode==0)
  127.             {
  128.                 /* Occupy correct controller */
  129.                 controller=(int)servo/SSC_MAX_SERVOS;
  130.                 if(!ssc_OccupyController(controller,rate,range))
  131.                 {
  132.                     /* Occupy servo */
  133.                     if(!ssc_OccupyServo(servo,servorange))
  134.                     {
  135.                         /* Set new servo position */
  136.                         if(ssc_SetAPosition(servo,position))
  137.                         {
  138.                             sprintf(msgstring,message[MSG_INVSERVOPOS],argv[0]);
  139.                             printf("%s\n",msgstring);
  140.                             errcode=20;
  141.                         }
  142.  
  143.                         /* Free servo */
  144.                         ssc_FreeServo(servo);
  145.                     }
  146.                     /* Not a valid servo */
  147.                     else
  148.                     {
  149.                         sprintf(msgstring,message[MSG_INVSERVOID],argv[0]);
  150.                         printf("%s\n",msgstring);
  151.                         errcode=20;
  152.                     }
  153.  
  154.                     /* Free controller */
  155.                     ssc_FreeController(controller);
  156.                 }
  157.                 /* Not a valid controller */
  158.                 else
  159.                 {
  160.                     sprintf(msgstring,message[MSG_INVSERVOID],argv[0]);
  161.                     printf("%s\n",msgstring);
  162.                     errcode=20;
  163.                 }
  164.             }
  165.         }
  166.         /* Missing argument */
  167.         else
  168.         {
  169.             sprintf(msgstring,message[MSG_ARGMISSING],argv[0]);
  170.             printf("%s\n",msgstring);
  171.             errcode=20;
  172.         }
  173.  
  174.         /* Close library */
  175.         CloseLibrary((struct Library *)MiniSSCBase);
  176.     }
  177.     /* minissc.library not available */
  178.     else
  179.     {
  180.         sprintf(msgstring,message[MSG_CANTOPEN],argv[0],SSC_NAME);
  181.         printf("%s\n",msgstring);
  182.         errcode=20;
  183.     }
  184.  
  185.     return errcode;
  186. }